#include "data_collect.h"

UdpSend::UdpSend()
{
    subNav = nh.subscribe("nav_msg", 1, &UdpSend::NavCallBack, this);

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if(sockfd == -1){
        perror("socket() error");
        exit(1);
    }

    addr_len = sizeof(addr);
    memset(&addr, 0, addr_len);
    addr.sin_family = AF_INET;
    addr.sin_port = htons(UDPPORT);
    addr.sin_addr.s_addr = htonl(INADDR_ANY);
    addr.sin_addr.s_addr = inet_addr(SERVERIP);

    send_data = (SendData*)sendline;
    send_data->msg_id = 0x00010001;
    send_data->msg_index = m_index;
    send_data->len = 28;
    send_data->team_id = 0x0205;
    ROS_INFO("Connect to %s:%d.", SERVERIP, UDPPORT);
}

UdpSend::~UdpSend()
{
    close(sockfd);
}

void UdpSend::NavCallBack(const autodrive_msgs::NavPacket::Ptr &Navmsg)
{
    uint32_t today_time = (uint32_t)((ros::Time::now().toNSec()*1000000 + 8*60*60*1000) % 24*60*60*1000);
    send_data->timestamp = today_time;

    send_data->longitude = (uint32_t)Navmsg->position.longitude / 0.000001;
    send_data->latitude = (uint32_t)Navmsg->position.latitude / 0.000001;
    send_data->altitude = (uint32_t)Navmsg->position.altitude;
}

void UdpSend::sendData()
{
    uint32_t crc_ = getCRC(sendline, 36);
    send_data->crc = crc_;

    sendto(sockfd, sendline, sizeof(sendline), 0, (struct sockaddr *) &addr, addr_len);

    m_index += 1;
    send_data->msg_index = m_index;
}

uint32_t getCRC(char* buf, uint16_t len)
{
    unsigned int i,j;
    unsigned short crc,flag;
    crc=0x0000;
    for(i=12; i<len; i++)
    {
        crc ^= (((unsigned short)buf[i])<<8);
        for(j=0; j<8; j++)
        {
            flag = crc&0x8000;
            crc <<= 1;
            if(flag) 
            {
                crc &= 0xfffe;
                crc ^= 0x8005;
            }
        }
    }
    return crc;
}